/****************************************************************************************************/
/**
\file       SchM.c
\brief      Scheduler module main functionality of the scheduler
\date       08/03/2014
*/
/****************************************************************************************************/

#include <hidef.h>      /* common defines and macros */
#include "SchM.h"
#include "Mcu.h"
#include "Gpt.h"
#include "OS_TaskM.h"
#include "OS_Task.h"
                   
                   

static u8  schM_counter;
static u8 tickChanged=FALSE;
static u16 schM_taskNumber;
static u8 schM_started=CNF_FALSE;
OS_TaskControlBlockInformationType *schm_task_status;
TaskType currentRunning;
tCallbackFunction callback;


/**************************************************************************************
* - Function      SchM_Init
* - Description   Function to init scheduler information, os tick and corresponding clocks.
  ` Author        Equipo 1
* - Param         schM_Config Pointer with task information
* - Return        void
****************************************************************************************/
void SchM_Init(const OS_TaskConfigType *schM_Config) 
{
  u8 taskIndex;
  u8 priorityIndex;
  OS_TaskType * taskConfigurations;
  schM_counter = 0;   
  currentRunning = INVALID_TASK; 
  schM_taskNumber = schM_Config->taskNumber;
  

  /*Initialize taskQueue structure*/
  for(priorityIndex = 0; priorityIndex < CNF_MAX_PRIORITY+1 ;priorityIndex++) {    
     taskQueue[priorityIndex].unqueuIndex = 0;                      
     taskQueue[priorityIndex].queueIndex = 0;
     /*Allocate memory for each priority*/
      taskQueue[priorityIndex].taskQueue =(OS_TaskQueuedType *) MemAlloc_NearReserve(sizeof(OS_TaskQueuedType) * (CNF_TASK_QUEUE_DEEP_PER_PRIORITY+1)); 
  } 
    
  /*Allocate memory for task control block*/ 
  schm_task_status = (OS_TaskControlBlockInformationType *) MemAlloc_NearReserve(sizeof(OS_TaskControlBlockInformationType) * schM_taskNumber);
  /*Initialize task control block structure using task configuration structure*/
  taskConfigurations= schM_Config->taskConfig;
  for(taskIndex = 0; taskIndex< schM_Config->taskNumber; taskIndex++ ) { 
  
      callback = taskConfigurations->task_function;
      schm_task_status[taskIndex].taskfunction = taskConfigurations->task_function;
      schm_task_status[taskIndex].mask = taskConfigurations->mask;
      schm_task_status[taskIndex].offset = taskConfigurations->offset;  
      /*By default set task in suspended mode*/ 
      schm_task_status[taskIndex].taskState = TASK_SUSPENDED;
      schm_task_status[taskIndex].id =taskConfigurations->id; 
      schm_task_status[taskIndex].priority = taskConfigurations->priority; 
      taskConfigurations++;   
  }
  
  Mcu_Init(); /*Already done by Start12.c code */
  Mcu_InitClock();           /* Initialize PLL */
  Mcu_DistributePllClock();  /* Set PLL as source Clock */
  
}                    
 
 
/**************************************************************************************
* - Function      SchM_DeInit
* - Description   Function to deinit scheduler and its os tick
* - Author        Equipo 1
* - Param         void
* - Return        void
****************************************************************************************/ 
void SchM_DeInit(void) 
{       
     schM_started=CNF_FALSE;           
     Gpt_DisableNotification(GPT_CHANNEL_0); /* Disable OS Tick callback */ 
     Gpt_StopTimer( GPT_CHANNEL_0);          /* Stop OS Tick timer */ 
}


/**************************************************************************************
* - Function      SchM_Start
* - Description   Function to start scheduler and its os tick
* - Author        Equipo 1
* - Param         void
* - Return        void
****************************************************************************************/
void SchM_Start(void)
{
  Gpt_Init(&mcal_gpt_configuration);     /* Initialize Gpt driver */
  Gpt_StartTimer( GPT_CHANNEL_0);        /* Start Gpt Channel 0 */
  Gpt_EnableNotification(GPT_CHANNEL_0); /* Enable notifications of Channel 0 */
  schM_started=CNF_TRUE;
  
  
  for(;;) 
  {
    _FEED_COP(); /* feeds the dog */
    SchM_Background();
  } /* loop forever */
}

/**************************************************************************************
* - Function      SchM_Background
* - Description   Function that must be running all time to execute corresponding task
* - Author        Equipo 1
* - Param         void
* - Return        void
****************************************************************************************/
void SchM_Background(void)
{
  if(schM_started && (TRUE ==tickChanged)) {
    
    SchM_Dispatcher();
    tickChanged = FALSE;      
  }
}


/**************************************************************************************
* - Function      SchM_OsTick
* - Description   Function used as OS tick to enable tasks according to its mask and offset
* - Author        Equipo 1
* - Param         void
* - Return        void
****************************************************************************************/
void SchM_OsTick(void)
{
  TaskType taskIndex;
  if(CNF_TRUE ==schM_started  ) 
  { 
    /* Debug PIN for CPU load meaurement */
    PORTB_PB2 = 1; 
                             
    /* Scan all tasks in teh task table  */
    for(taskIndex = 0; taskIndex< schM_taskNumber;  taskIndex++ ) 
    {
        /* Activate corresponding tasks based in his mask and offset */
        if (schm_task_status[taskIndex].offset == (schM_counter & schm_task_status[taskIndex].mask)) 
        {
             (void)ActivateTask(taskIndex);            
        }
     }
    /* Increase OS Tick counter */
    schM_counter++;
     tickChanged = TRUE;   
     
     /* Debug PIN for CPU load meaurement */
     PORTB_PB2 = 0;
  } 
}

/**************************************************************************************
* - Function      SchM_Dispatcher
* - Description   Function used as OS dispatcher
* - Author        Equipo 1
* - Param         void
* - Return        void
****************************************************************************************/
void SchM_Dispatcher(void) 
{   
    u8 priorityIndex;
    u8 currentQueueIndex; 
    u8 taskfound;
    /* Scan all tasks in teh task table  */
    do
    { 
      /*By default is not an activate task*/
      taskfound = FALSE;
      /*Set current priority as maximum */
      priorityIndex =CNF_MAX_PRIORITY; 
      
      do
      {  
      
          priorityIndex--;
        /*Get index that must be unqueue*/
         currentQueueIndex = taskQueue[priorityIndex].unqueuIndex;
         
         /*Task to be unqueue is not null*/
         if( NULL != taskQueue[priorityIndex].taskQueue[currentQueueIndex].task_function ) 
         {
            taskfound = TRUE;
            /*Store current running task*/
            currentRunning = taskQueue[priorityIndex].taskQueue[currentQueueIndex].id;
            
              /*Set current task status as running*/
            schm_task_status[currentRunning].taskState = TASK_RUNNING;
            
             /*Execute task callback*/
            taskQueue[priorityIndex].taskQueue[currentQueueIndex].task_function();

            
             /*Clean queue structure*/
            taskQueue[priorityIndex].taskQueue[currentQueueIndex].task_function = NULL;  
            
            /*Set current task ID as invalid*/
            taskQueue[priorityIndex].taskQueue[currentQueueIndex].id =INVALID_TASK;
            
            
            taskQueue[priorityIndex].unqueuIndex++;
            /*Reset unqueued index if necessary*/
            if(taskQueue[priorityIndex].unqueuIndex == CNF_TASK_QUEUE_DEEP_PER_PRIORITY ) 
            {
              taskQueue[priorityIndex].unqueuIndex=0;
            }
         } 
        }while( priorityIndex);  /*Find in all tasks queues*/ 
      }while(taskfound);   /*Search again if task was found*/ 
}






